// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2019
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// File Version: 3.0.0 (2016/06/19)

#pragma once

#include <Mathematics/GteDelaunay2.h>
#include <Mathematics/GteContScribeCircle2.h>
#include <Mathematics/GteDistPointAlignedBox.h>

// Quadratic interpolation of a network of triangles whose vertices are of
// the form (x,y,f(x,y)).  This code is an implementation of the algorithm
// found in
//
//   Zoltan J. Cendes and Steven H. Wong,
//   C1 quadratic interpolation over arbitrary point sets,
//   IEEE Computer Graphics & Applications,
//   pp. 8-16, 1987
//
// The TriangleMesh interface must support the following:
//   int GetNumVertices() const;
//   int GetNumTriangles() const;
//   Vector2<Real> const* GetVertices() const;
//   int const* GetIndices() const;
//   bool GetVertices(int, std::array<Vector2<Real>, 3>&) const;
//   bool GetIndices(int, std::array<int, 3>&) const;
//   bool GetAdjacencies(int, std::array<int, 3>&) const;
//   bool GetBarycentrics(int, Vector2<Real> const&,
//      std::array<Real, 3>&) const;
//   int GetContainingTriangle(Vector2<Real> const&) const;

namespace gte
{

template <typename Real, typename TriangleMesh>
class IntpQuadraticNonuniform2
{
public:
    // Construction.
    //
    // The first constructor requires only F and a measure of the rate of
    // change of the function values relative to changes in the spatial
    // variables.  The df/dx and df/dy values are estimated at the sample
    // points using mesh normals and spatialDelta.
    //
    // The second constructor requires you to specify function values F and
    // first-order partial derivative values df/dx and df/dy.

    IntpQuadraticNonuniform2(TriangleMesh const& mesh, Real const* F,
        Real spatialDelta);

    IntpQuadraticNonuniform2(TriangleMesh const& mesh, Real const* F,
        Real const* FX, Real const* FY);

    // Quadratic interpolation.  The return value is 'true' if and only if the
    // input point is in the convex hull of the input vertices, in which case
    // the interpolation is valid.
    bool operator()(Vector2<Real> const & P, Real& F, Real& FX, Real& FY)
        const;

private:
    void EstimateDerivatives(Real spatialDelta);
    void ProcessTriangles();
    void ComputeCrossEdgeIntersections(int t);
    void ComputeCoefficients(int t);

    class TriangleData
    {
    public:
        Vector2<Real> center;
        Vector2<Real> intersect[3];
        Real coeff[19];
    };

    class Jet
    {
    public:
        Real F, FX, FY;
    };

    TriangleMesh const* mMesh;
    Real const* mF;
    Real const* mFX;
    Real const* mFY;
    std::vector<Real> mFXStorage;
    std::vector<Real> mFYStorage;
    std::vector<TriangleData> mTData;
};


template <typename Real, typename TriangleMesh>
IntpQuadraticNonuniform2<Real, TriangleMesh>::IntpQuadraticNonuniform2(
    TriangleMesh const& mesh, Real const* F, Real spatialDelta)
    :
    mMesh(&mesh),
    mF(F),
    mFX(nullptr),
    mFY(nullptr)
{
    EstimateDerivatives(spatialDelta);
    ProcessTriangles();
}

template <typename Real, typename TriangleMesh>
IntpQuadraticNonuniform2<Real, TriangleMesh>::IntpQuadraticNonuniform2(
    TriangleMesh const& mesh, Real const* F, Real const* FX, Real const* FY)
    :
    mMesh(&mesh),
    mF(F),
    mFX(FX),
    mFY(FY)
{
    ProcessTriangles();
}

template <typename Real, typename TriangleMesh>
bool IntpQuadraticNonuniform2<Real, TriangleMesh>::operator()(
    Vector2<Real> const& P, Real& F, Real& FX, Real& FY) const
{
    int t = mMesh->GetContainingTriangle(P);
    if (t == -1)
    {
        // The point is outside the triangulation.
        return false;
    }

    // Get the vertices of the triangle.
    std::array<Vector2<Real>, 3> V;
    mMesh->GetVertices(t, V);

    // Get the additional information for the triangle.
    TriangleData const& tData = mTData[t];

    // Determine which of the six subtriangles contains the target point.
    // Theoretically, P must be in one of these subtriangles.
    Vector2<Real> sub0 = tData.center;
    Vector2<Real> sub1;
    Vector2<Real> sub2 = tData.intersect[2];
    Vector3<Real> bary;
    int index;

    Real const zero = (Real)0, one = (Real)1;
    AlignedBox3<Real> barybox({ zero, zero, zero }, { one, one, one });
    DCPQuery<Real, Vector3<Real>, AlignedBox3<Real>> pbQuery;
    int minIndex = 0;
    Real minDistance = (Real)-1;
    Vector3<Real> minBary;
    Vector2<Real> minSub0, minSub1, minSub2;

    for (index = 1; index <= 6; ++index)
    {
        sub1 = sub2;
        if (index % 2)
        {
            sub2 = V[index / 2];
        }
        else
        {
            sub2 = tData.intersect[index / 2 - 1];
        }

        bool valid = ComputeBarycentrics(P, sub0, sub1, sub2, &bary[0]);
        if (valid
            && zero <= bary[0] && bary[0] <= one
            && zero <= bary[1] && bary[1] <= one
            && zero <= bary[2] && bary[2] <= one)
        {
            // P is in triangle <Sub0,Sub1,Sub2>
            break;
        }

        // When computing with floating-point arithmetic, rounding errors
        // can cause us to reach this code when, theoretically, the point
        // is in the subtriangle.  Keep track of the (b0,b1,b2) that is
        // closest to the barycentric cube [0,1]^3 and choose the triangle
        // corresponding to it when all 6 tests previously fail.
        Real distance = pbQuery(bary, barybox).distance;
        if (minIndex == 0 || distance < minDistance)
        {
            minDistance = distance;
            minIndex = index;
            minBary = bary;
            minSub0 = sub0;
            minSub1 = sub1;
            minSub2 = sub2;
        }
    }

    // If the subtriangle was not found, rounding errors caused problems.
    // Choose the barycentric point closest to the box.
    if (index > 6)
    {
        index = minIndex;
        bary = minBary;
        sub0 = minSub0;
        sub1 = minSub1;
        sub2 = minSub2;
    }

    // Fetch Bezier control points.
    Real bez[6] =
    {
        tData.coeff[0],
        tData.coeff[12 + index],
        tData.coeff[13 + (index % 6)],
        tData.coeff[index],
        tData.coeff[6 + index],
        tData.coeff[1 + (index % 6)]
    };

    // Evaluate Bezier quadratic.
    F = bary[0] * (bez[0] * bary[0] + bez[1] * bary[1] + bez[2] * bary[2]) +
        bary[1] * (bez[1] * bary[0] + bez[3] * bary[1] + bez[4] * bary[2]) +
        bary[2] * (bez[2] * bary[0] + bez[4] * bary[1] + bez[5] * bary[2]);

    // Evaluate barycentric derivatives of F.
    Real FU = ((Real)2)*(bez[0] * bary[0] + bez[1] * bary[1] +
        bez[2] * bary[2]);
    Real FV = ((Real)2)*(bez[1] * bary[0] + bez[3] * bary[1] +
        bez[4] * bary[2]);
    Real FW = ((Real)2)*(bez[2] * bary[0] + bez[4] * bary[1] +
        bez[5] * bary[2]);
    Real duw = FU - FW;
    Real dvw = FV - FW;

    // Convert back to (x,y) coordinates.
    Real m00 = sub0[0] - sub2[0];
    Real m10 = sub0[1] - sub2[1];
    Real m01 = sub1[0] - sub2[0];
    Real m11 = sub1[1] - sub2[1];
    Real inv = ((Real)1) / (m00*m11 - m10*m01);

    FX = inv*(m11*duw - m10*dvw);
    FY = inv*(m00*dvw - m01*duw);
    return true;
}

template <typename Real, typename TriangleMesh>
void IntpQuadraticNonuniform2<Real, TriangleMesh>::EstimateDerivatives(
    Real spatialDelta)
{
    int numVertices = mMesh->GetNumVertices();
    Vector2<Real> const* vertices = mMesh->GetVertices();
    int numTriangles = mMesh->GetNumTriangles();
    int const* indices = mMesh->GetIndices();

    mFXStorage.resize(numVertices);
    mFYStorage.resize(numVertices);
    std::vector<Real> FZ(numVertices);
    std::fill(mFXStorage.begin(), mFXStorage.end(), (Real)0);
    std::fill(mFYStorage.begin(), mFYStorage.end(), (Real)0);
    std::fill(FZ.begin(), FZ.end(), (Real)0);

    mFX = &mFXStorage[0];
    mFY = &mFYStorage[0];

    // Accumulate normals at spatial locations (averaging process).
    for (int t = 0; t < numTriangles; ++t)
    {
        // Get three vertices of triangle.
        int v0 = *indices++;
        int v1 = *indices++;
        int v2 = *indices++;

        // Compute normal vector of triangle (with positive z-component).
        Real dx1 = vertices[v1][0] - vertices[v0][0];
        Real dy1 = vertices[v1][1] - vertices[v0][1];
        Real dz1 = mF[v1] - mF[v0];
        Real dx2 = vertices[v2][0] - vertices[v0][0];
        Real dy2 = vertices[v2][1] - vertices[v0][1];
        Real dz2 = mF[v2] - mF[v0];
        Real nx = dy1*dz2 - dy2*dz1;
        Real ny = dz1*dx2 - dz2*dx1;
        Real nz = dx1*dy2 - dx2*dy1;
        if (nz < (Real)0)
        {
            nx = -nx;
            ny = -ny;
            nz = -nz;
        }

        mFXStorage[v0] += nx;  mFYStorage[v0] += ny;  FZ[v0] += nz;
        mFXStorage[v1] += nx;  mFYStorage[v1] += ny;  FZ[v1] += nz;
        mFXStorage[v2] += nx;  mFYStorage[v2] += ny;  FZ[v2] += nz;
    }

    // Scale the normals to form (x,y,-1).
    for (int i = 0; i < numVertices; ++i)
    {
        if (FZ[i] != (Real)0)
        {
            Real inv = -spatialDelta / FZ[i];
            mFXStorage[i] *= inv;
            mFYStorage[i] *= inv;
        }
        else
        {
            mFXStorage[i] = (Real)0;
            mFYStorage[i] = (Real)0;
        }
    }
}

template <typename Real, typename TriangleMesh>
void IntpQuadraticNonuniform2<Real, TriangleMesh>::ProcessTriangles()
{
    // Add degenerate triangles to boundary triangles so that interpolation
    // at the boundary can be treated in the same way as interpolation in
    // the interior.

    // Compute centers of inscribed circles for triangles.
    Vector2<Real> const* vertices = mMesh->GetVertices();
    int numTriangles = mMesh->GetNumTriangles();
    int const* indices = mMesh->GetIndices();
    mTData.resize(numTriangles);
    int t;
    for (t = 0; t < numTriangles; ++t)
    {
        int v0 = *indices++;
        int v1 = *indices++;
        int v2 = *indices++;
        Circle2<Real> circle;
        Inscribe(vertices[v0], vertices[v1], vertices[v2], circle);
        mTData[t].center = circle.center;
    }

    // Compute cross-edge intersections.
    for (t = 0; t < numTriangles; ++t)
    {
        ComputeCrossEdgeIntersections(t);
    }

    // Compute Bezier coefficients.
    for (t = 0; t < numTriangles; ++t)
    {
        ComputeCoefficients(t);
    }
}

template <typename Real, typename TriangleMesh>
void IntpQuadraticNonuniform2<Real, TriangleMesh>::
ComputeCrossEdgeIntersections(int t)
{
    // Get the vertices of the triangle.
    std::array<Vector2<Real>, 3> V;
    mMesh->GetVertices(t, V);

    // Get the centers of adjacent triangles.
    TriangleData& tData = mTData[t];
    std::array<int, 3> adjacencies;
    mMesh->GetAdjacencies(t, adjacencies);
    for (int j0 = 2, j1 = 0; j1 < 3; j0 = j1++)
    {
        int a = adjacencies[j0];
        if (a >= 0)
        {
            // Get center of adjacent triangle's inscribing circle.
            Vector2<Real> U = mTData[a].center;
            Real m00 = V[j0][1] - V[j1][1];
            Real m01 = V[j1][0] - V[j0][0];
            Real m10 = tData.center[1] - U[1];
            Real m11 = U[0] - tData.center[0];
            Real r0 = m00*V[j0][0] + m01*V[j0][1];
            Real r1 = m10*tData.center[0] + m11*tData.center[1];
            Real invDet = ((Real)1) / (m00*m11 - m01*m10);
            tData.intersect[j0][0] = (m11*r0 - m01*r1)*invDet;
            tData.intersect[j0][1] = (m00*r1 - m10*r0)*invDet;
        }
        else
        {
            // No adjacent triangle, use center of edge.
            tData.intersect[j0] = ((Real)0.5)*(V[j0] + V[j1]);
        }
    }
}

template <typename Real, typename TriangleMesh>
void IntpQuadraticNonuniform2<Real, TriangleMesh>::ComputeCoefficients(int t)
{
    // Get the vertices of the triangle.
    std::array<Vector2<Real>, 3> V;
    mMesh->GetVertices(t, V);

    // Get the additional information for the triangle.
    TriangleData& tData = mTData[t];

    // Get the sample data at main triangle vertices.
    std::array<int, 3> indices;
    mMesh->GetIndices(t, indices);
    Jet jet[3];
    for (int j = 0; j < 3; ++j)
    {
        int k = indices[j];
        jet[j].F = mF[k];
        jet[j].FX = mFX[k];
        jet[j].FY = mFY[k];
    }

    // Get centers of adjacent triangles.
    std::array<int, 3> adjacencies;
    mMesh->GetAdjacencies(t, adjacencies);
    Vector2<Real> U[3];
    for (int j0 = 2, j1 = 0; j1 < 3; j0 = j1++)
    {
        int a = adjacencies[j0];
        if (a >= 0)
        {
            // Get center of adjacent triangle's circumscribing circle.
            U[j0] = mTData[a].center;
        }
        else
        {
            // No adjacent triangle, use center of edge.
            U[j0] = ((Real)0.5)*(V[j0] + V[j1]);
        }
    }

    // Compute intermediate terms.
    std::array<Real, 3> cenT, cen0, cen1, cen2;
    mMesh->GetBarycentrics(t, tData.center, cenT);
    mMesh->GetBarycentrics(t, U[0], cen0);
    mMesh->GetBarycentrics(t, U[1], cen1);
    mMesh->GetBarycentrics(t, U[2], cen2);

    Real alpha = (cenT[1] * cen1[0] - cenT[0] * cen1[1])
        / (cen1[0] - cenT[0]);
    Real beta = (cenT[2] * cen2[1] - cenT[1] * cen2[2])
        / (cen2[1] - cenT[1]);
    Real gamma = (cenT[0] * cen0[2] - cenT[2] * cen0[0])
        / (cen0[2] - cenT[2]);
    Real oneMinusAlpha = (Real)1 - alpha;
    Real oneMinusBeta = (Real)1 - beta;
    Real oneMinusGamma = (Real)1 - gamma;

    Real tmp, A[9], B[9];

    tmp = cenT[0] * V[0][0] + cenT[1] * V[1][0] + cenT[2] * V[2][0];
    A[0] = ((Real)0.5)*(tmp - V[0][0]);
    A[1] = ((Real)0.5)*(tmp - V[1][0]);
    A[2] = ((Real)0.5)*(tmp - V[2][0]);
    A[3] = ((Real)0.5)*beta*(V[2][0] - V[0][0]);
    A[4] = ((Real)0.5)*oneMinusGamma*(V[1][0] - V[0][0]);
    A[5] = ((Real)0.5)*gamma*(V[0][0] - V[1][0]);
    A[6] = ((Real)0.5)*oneMinusAlpha*(V[2][0] - V[1][0]);
    A[7] = ((Real)0.5)*alpha*(V[1][0] - V[2][0]);
    A[8] = ((Real)0.5)*oneMinusBeta*(V[0][0] - V[2][0]);

    tmp = cenT[0] * V[0][1] + cenT[1] * V[1][1] + cenT[2] * V[2][1];
    B[0] = ((Real)0.5)*(tmp - V[0][1]);
    B[1] = ((Real)0.5)*(tmp - V[1][1]);
    B[2] = ((Real)0.5)*(tmp - V[2][1]);
    B[3] = ((Real)0.5)*beta*(V[2][1] - V[0][1]);
    B[4] = ((Real)0.5)*oneMinusGamma*(V[1][1] - V[0][1]);
    B[5] = ((Real)0.5)*gamma*(V[0][1] - V[1][1]);
    B[6] = ((Real)0.5)*oneMinusAlpha*(V[2][1] - V[1][1]);
    B[7] = ((Real)0.5)*alpha*(V[1][1] - V[2][1]);
    B[8] = ((Real)0.5)*oneMinusBeta*(V[0][1] - V[2][1]);

    // Compute Bezier coefficients.
    tData.coeff[2] = jet[0].F;
    tData.coeff[4] = jet[1].F;
    tData.coeff[6] = jet[2].F;

    tData.coeff[14] = jet[0].F + A[0] * jet[0].FX + B[0] * jet[0].FY;
    tData.coeff[7] = jet[0].F + A[3] * jet[0].FX + B[3] * jet[0].FY;
    tData.coeff[8] = jet[0].F + A[4] * jet[0].FX + B[4] * jet[0].FY;
    tData.coeff[16] = jet[1].F + A[1] * jet[1].FX + B[1] * jet[1].FY;
    tData.coeff[9] = jet[1].F + A[5] * jet[1].FX + B[5] * jet[1].FY;
    tData.coeff[10] = jet[1].F + A[6] * jet[1].FX + B[6] * jet[1].FY;
    tData.coeff[18] = jet[2].F + A[2] * jet[2].FX + B[2] * jet[2].FY;
    tData.coeff[11] = jet[2].F + A[7] * jet[2].FX + B[7] * jet[2].FY;
    tData.coeff[12] = jet[2].F + A[8] * jet[2].FX + B[8] * jet[2].FY;

    tData.coeff[5] = alpha*tData.coeff[10] + oneMinusAlpha*tData.coeff[11];
    tData.coeff[17] = alpha*tData.coeff[16] + oneMinusAlpha*tData.coeff[18];
    tData.coeff[1] = beta*tData.coeff[12] + oneMinusBeta*tData.coeff[7];
    tData.coeff[13] = beta*tData.coeff[18] + oneMinusBeta*tData.coeff[14];
    tData.coeff[3] = gamma*tData.coeff[8] + oneMinusGamma*tData.coeff[9];
    tData.coeff[15] = gamma*tData.coeff[14] + oneMinusGamma*tData.coeff[16];
    tData.coeff[0] = cenT[0] * tData.coeff[14] + cenT[1] * tData.coeff[16] +
        cenT[2] * tData.coeff[18];
}


}
